// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SDFIO.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Geometry.SDF
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using System.Runtime.InteropServices;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Input and output methods for a signed distance function
    /// </summary>
    public class SDFIO
    {
        /// <summary>
        /// Saves the signed distance function to the specified directory with the specified name
        /// </summary>
        /// <param name="sdf">The signed distance function to save</param>
        /// <param name="directory">The directory to save the signed distance function in</param>
        /// <param name="name">The name of the file to create</param>
        public static void SaveSDF(SDF sdf, string directory, string name)
        {
            string file = Path.Combine(directory, name);
            BinaryWriter writer = new BinaryWriter(File.Open(file, FileMode.Create));

            writer.Write(sdf.Width);
            writer.Write(sdf.Height);
            writer.Write(sdf.Depth);
            writer.Write(sdf.Resolution);

            byte[] data = new byte[sdf.Data.Length * sizeof(double)];
            Buffer.BlockCopy(sdf.Data, 0, data, 0, data.Length);

            writer.Write(data);
            writer.Close();
        }

        /// <summary>
        /// Loads a signed distance function from a specified file
        /// </summary>
        /// <param name="directory">The directory to load the signed distance function from</param>
        /// <param name="name">The name of the file to read</param>
        /// <returns>The loaded signed distance function</returns>
        public static SDF LoadSDF(string directory, string name)
        {
            string file = Path.Combine(directory, name);

            BinaryReader reader = new BinaryReader(File.Open(file, FileMode.Open));

            int width = reader.ReadInt32();
            int height = reader.ReadInt32();
            int depth = reader.ReadInt32();
            double resolution = reader.ReadDouble();

            byte[] data = reader.ReadBytes(width * height * depth * sizeof(double));
            double[] sdfData = new double[width * height * depth];

            Buffer.BlockCopy(data, 0, sdfData, 0, data.Length);

            SDF sdf = new SDF(width, height, depth, resolution, sdfData);

            return sdf;
        }

        /// <summary>
        /// A method that creates a mesh out of the point cloud of the surface crossings for easy visualization of the signed distance function
        /// </summary>
        /// <param name="sdf">The signed distance function</param>
        /// <param name="directory">The directory to save the cloud</param>
        /// <param name="name">The name of the file to save</param>
        /// <param name="threshold">The value for which the signed distance function is considered to be a "surface"</param>
        public static void SavePlyToFileFromSDF(SDF sdf, string directory, string name, double threshold = 0.5)
        {
            List<Tuple<Vector3, Vector3>> surfaces = SDFIO.GetSurfaceFromSDF(sdf, threshold);
            SDFIO.SavePlyToFile(surfaces, Path.Combine(directory, name), 255, 0, 0);
        }

        /// <summary>
        /// A method that creates a point cloud of the surface crossings for easy visualization of the signed distance function
        /// </summary>
        /// <param name="sdf">The signed distance function</param>
        /// <param name="threshold">The value for which the signed distance function is considered to be a "surface"</param>
        /// <returns> List of tuples of surface points and their normal directions based on the input threshold </returns>
        public static List<Tuple<Vector3, Vector3>> GetSurfaceFromSDF(SDF sdf, double threshold = 0.5)
        {
            List<Tuple<Vector3, Vector3>> surfaces = new List<Tuple<Vector3, Vector3>>();

            for (int i = 0; i < sdf.Height; i += 1)
            {
                for (int j = 0; j < sdf.Width; j += 1)
                {
                    for (int k = 0; k < sdf.Depth; k += 1)
                    {
                        if (Math.Abs(sdf[j, i, k]) < threshold)
                        {
                            Vector3 point = sdf.Get3DPoint(j, i, k);
                            Vector3 normal = -Vector3.Normalize(sdf.Gradient(point)) * sdf.Volume() / 10.0f;
                            surfaces.Add(new Tuple<Vector3, Vector3>(point, normal));
                        }
                    }
                }
            }

            return surfaces;
        }

        /// <summary>
        /// Creates a ply file of the detected feature points for a given frame (temporary)
        /// </summary>
        /// <param name="points">The list of points</param>
        /// <param name="filename">name of the file to create</param>
        /// <param name="red">red color</param>
        /// <param name="green">green color</param>
        /// <param name="blue">blue color</param>
        private static void SavePlyToFile(List<Tuple<Vector3, Vector3>> points, string filename, int red, int green, int blue)
        {
            StreamWriter writer = new StreamWriter(filename);
            writer.Write(
                            "ply\n" +
                            "format ascii 1.0\n" +
                            "element vertex {0}\n" +
                            "property float32 x\n" +
                            "property float32 y\n" +
                            "property float32 z\n" +
                            "property float32 nx\n" +
                            "property float32 ny\n" +
                            "property float32 nz\n" +
                            "property uchar red\n" +
                            "property uchar green\n" +
                            "property uchar blue\n" +
                            "end_header\n",
                            points.Count);

            for (int i = 0; i < points.Count; i++)
            {
                Vector3 point = points[i].Item1;
                Vector3 normal = points[i].Item2;
                if (double.IsNaN(normal.X))
                {
                    normal = new Vector3(0, 0, 0);
                }

                writer.WriteLine("{0} {1} {2} {3} {4} {5} {6} {7} {8}", point.X, point.Y, point.Z, normal.X, normal.Y, normal.Z, red, green, blue);
            }

            writer.Close();
        }
    }
}
